/* Copyright (c) 2011 Alexandre Défossez
 * This file is part of HINAPT.
 *
 * HINAPT is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * HINAPT is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with HINAPT.  If not, see <http://www.gnu.org/licenses/>.
 */


#include "HeadPosition.hpp"

HeadPosition::HeadPosition(double screenHeight,
                 double eyesDistance, double camAngle,
                 int camWidth, int camHeight, bool camIsAbove,
                 double field, double scaling
): screenHeight(screenHeight),
   eyesDistance(eyesDistance), camAngle(camAngle),
   camWidth(camWidth), camHeight(camHeight), camIsAbove(camIsAbove),
   field(field), scaling(scaling), radPerPix(field/camWidth) {}

Point3d HeadPosition::operator()(vector<Point2f> &eyes, Mat &A){
  double d = distance<Point2f>(eyes[0], eyes[1]);
  double angle = radPerPix * d/2;
  double headZ = scaling * ((eyesDistance/2)/tan(angle));
  double x = (eyes[0].x + eyes[1].x)/2;
  double y = (eyes[0].y + eyes[1].y)/2;
  double headX = sin(radPerPix * (x - camWidth/2)) * headZ;
  double headY;
  double verticalAngle = (y - camHeight/2) * radPerPix + camAngle;
  if (camIsAbove)
    headY = screenHeight/2 - sin(verticalAngle) * headZ;
  else
    headY = -screenHeight/2 - sin(verticalAngle) * headZ;

  // Calcul de la matrice A d'aide au suivi

  Point2f leftEye, rightEye;
  if (eyes[0].x < eyes[1].x){
    leftEye = eyes[0];
    rightEye = eyes[1];
  } else {
    leftEye = eyes[1];
    rightEye = eyes[0];
  }
  double lambda = d/originalD;
  double theta = atan((leftEye.y - rightEye.y)/(rightEye.x - leftEye.x)) - originalTheta;
  A = (Mat_<double>(2, 2) << lambda * cos(theta),sin(theta),
                              -sin(theta), lambda * cos(theta));

  return Point3d(headX, headY, headZ);
}

void HeadPosition::setOriginalPositions(vector<Point2f> &eyes){
  Point2f leftEye, rightEye;
  if (eyes[0].x < eyes[1].x){
    leftEye = eyes[0];
    rightEye = eyes[1];
  } else {
    leftEye = eyes[1];
    rightEye = eyes[0];
  }
  originalTheta = atan((leftEye.y - rightEye.y)/(rightEye.x - leftEye.x));
  originalD = distance<Point2f>(eyes[0], eyes[1]);
}


